import rclpy
from rclpy.node import Node
from origincar_msg.msg import Sign  # 根据图片的消息类型
from std_msgs.msg import Int32
from geometry_msgs.msg import Twist
from ai_msgs.msg import PerceptionTargets   #把yolo的给弄下来

class StateMachine(Node):
    def __init__(self):
        super().__init__('state_machine_node')
        # 0为暂停，1为循迹
        self.state = Int32()  # 初始状态为暂停
        self.state.data = 0
        self.a = 0
        self.consecutive_count = 0  # 记录连续接收到3或4的次数
        self.qrcode_subscriber = self.create_subscription(
            Sign,
            '/sign_switch',
            self.listener_callback_qrcode_subscriber,
            10)
        # 创建一个订阅者，订阅 /sigh4return 话题，消息类型为 Int32
        self.sign4return = self.create_subscription(
            Int32,
            '/sign4return',
            self.listener_callback_sign4return,
            10)
        
        
        self.subscription_yolo = self.create_subscription( 
            PerceptionTargets, 
            'hobot_dnn_detection',  
            self.listener_yolo_callback, 
            10) 
        
        self.publisher_twist = self.create_publisher(Twist, '/cmd_vel', 10)  # 创建 Twist 消息的发布者
        self.publisher_state = self.create_publisher(Int32, 'state', 10)    #创建 state发布者
        self.publisher_foxglove = self.create_publisher(Int32, 'sign_foxglove', 10) # 创建发布者，发布到 /sign_foxglove.data 话题

    def listener_callback_qrcode_subscriber(self, msg):        
        if msg.sign_data in [3, 4]:
            self.consecutive_count += 1
        else:
            self.consecutive_count = 0

        if self.consecutive_count >= 2 and self.a == 0:
            self.state.data = 0  # 切换到暂停状态
            self.a =  1   #这里识别二维码实际上只需要识别一次就行了,a为flag
          
            twist_msg = Twist()
            twist_msg.linear.x = 0.00  # 示例：将 x 坐标复位
            twist_msg.angular.z = 0.00  # 使用 角速度复位
            
            # 发布 Twist 消息
            self.publisher_twist.publish(twist_msg)
            self.get_logger().info(f"已复位")
            self.publisher_state.publish(self.state)

        
        
    def listener_callback_sign4return(self,msg):
        foxglove_msg = Int32()
        # 回调函数，处理接收到的消息
        self.get_logger().info(f'收到消息: {msg.data}')
        if (msg.data == 0):
            self.get_logger().info(f'信号连接正常')
            
            foxglove_msg.data = 0      
             
        if (msg.data == -1):
            self.get_logger().info(f'重置里程计')
            
            foxglove_msg.data = -1
            
        if (msg.data == 6):
            self.get_logger().info(f'结束遥感')

            foxglove_msg.data = 6  

        if (msg.data == 5):
            self.get_logger().info(f'开始遥感')

            foxglove_msg.data = 5

         # 发布消息
        self.publisher_foxglove.publish(foxglove_msg)


        if msg.data == 6:
            self.state.data = 1 #结束遥感开始跑步
            self.a = 0
            self.get_logger().info(f"结束遥感")
        if msg.data == -1:
            self.state.data = 1 #重置里程计开始跑步
            self.a = 0
            self.get_logger().info(f"重置里程计")
        self.publisher_state.publish(self.state)
            
    def listener_yolo_callback(self,msg):
        pass
def main(args=None):
    rclpy.init(args=args)
    state_machine = StateMachine()
    rclpy.spin(state_machine)
    state_machine.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()



